#pragma once

// ===== C++ 标准库 =====
#include <atomic>
#include <cstddef>
#include <deque>
#include <memory>
#include <mutex>
#include <optional>
#include <vector>

// ===== ROS2/MoveIt/TF =====
#include <rclcpp/rclcpp.hpp>
#include <rcl_interfaces/msg/set_parameters_result.hpp>
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <std_msgs/msg/float64.hpp>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit_servo/servo_parameters.h>
#include <moveit_servo/servo.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

// ================== 默认参数（便于统一管理） ==================
constexpr double DEFAULT_KP_X = 2.8;
constexpr double DEFAULT_KI_X = 0.2;
constexpr double DEFAULT_KD_X = 0.14;
constexpr double DEFAULT_TARGET_DEPTH_M = 0.20;
constexpr double DEFAULT_ERROR_THRESHOLD_DEPTH_M = 0.02;
constexpr size_t DEFAULT_RAW_WIN_X = 15;
constexpr size_t DEFAULT_MED_WIN_X = 15;

constexpr double DEFAULT_KP = 0.0013;
constexpr double DEFAULT_KI = 0.0002;
constexpr double DEFAULT_KD = 0.0001;
constexpr double DEFAULT_ERROR_THRESHOLD_X = 9.0;
constexpr size_t DEFAULT_RAW_WIN_Y = 48;
constexpr size_t DEFAULT_MED_WIN_Y = 36;

constexpr double DEFAULT_KP_Z = 0.001;
constexpr double DEFAULT_KI_Z = 0.0001;
constexpr double DEFAULT_KD_Z = 0.0006;
constexpr double DEFAULT_ERROR_THRESHOLD_Y = 5.0;
constexpr size_t DEFAULT_RAW_WIN_Z = 15;
constexpr size_t DEFAULT_MED_WIN_Z = 15;

constexpr double DEFAULT_CONTROL_DT = 0.01;

// 阶段控制参数
constexpr double DEFAULT_SMALL_FORWARD_SPEED = 0.08; // 7~13.5 s 的前进速度 (m/s)
constexpr double DEFAULT_PHASE1_END = 10.0;          // 0 ~ 10 s：XYZ闭环
constexpr double DEFAULT_PHASE2_END = 13.0;          // 10 ~ 13 s：前进恒速
constexpr double DEFAULT_PHASE3_BEGIN = 13.5;        // > 13.5 s：停止并完成

// 每轴 PID 状态
struct PIDState {
  double i_term{0.0};
  std::optional<double> prev_error{};
};

namespace servo_visual {

class IBVSControl : public rclcpp::Node{
public:
  // 兼容两种构造：无参 & NodeOptions
  IBVSControl();
  explicit IBVSControl(const rclcpp::NodeOptions& options);

  // 初始化内部资源（订阅、发布、定时器、PSM/Servo 等）
  void initialize();

  // 外部查询是否结束
  bool isFinished() const;

private:
  // ================= 时钟/时间 =================
  rclcpp::Clock steady_clock_{RCL_STEADY_TIME};
  rclcpp::Time start_time_steady_;
  rclcpp::Time last_pid_time_steady_;

  // ================= 滤波窗口参数 =================
  size_t raw_win_x_{DEFAULT_RAW_WIN_X}, raw_win_y_{DEFAULT_RAW_WIN_Y}, raw_win_z_{DEFAULT_RAW_WIN_Z};
  size_t med_win_x_{DEFAULT_MED_WIN_X}, med_win_y_{DEFAULT_MED_WIN_Y}, med_win_z_{DEFAULT_MED_WIN_Z};

  // ================= 控制参数（Y/Z 通用 + X/Z 专用） =================
  // 说明：保留你原来三套 PID（通用/ X / Z），并且可在参数回调里在线修改
  double Kp_{DEFAULT_KP},  Ki_{DEFAULT_KI},  Kd_{DEFAULT_KD};       // 通用（例如 Y）
  double Kp_z_{DEFAULT_KP_Z}, Ki_z_{DEFAULT_KI_Z}, Kd_z_{DEFAULT_KD_Z};
  double Kp_x_{DEFAULT_KP_X}, Ki_x_{DEFAULT_KI_X}, Kd_x_{DEFAULT_KD_X};

  double target_depth_m_{DEFAULT_TARGET_DEPTH_M};
  double error_threshold_depth_m_{DEFAULT_ERROR_THRESHOLD_DEPTH_M};
  double control_dt_{DEFAULT_CONTROL_DT};
  double error_threshold_x_{DEFAULT_ERROR_THRESHOLD_X};
  double error_threshold_y_{DEFAULT_ERROR_THRESHOLD_Y};

  // 阶段控制参数
  double small_forward_speed_{DEFAULT_SMALL_FORWARD_SPEED};
  double phase1_end_{DEFAULT_PHASE1_END};
  double phase2_end_{DEFAULT_PHASE2_END};
  double phase3_begin_{DEFAULT_PHASE3_BEGIN};

  // ================= 在线参数回调 =================
  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_cb_handle_;

  // ================= PID 状态 =================
  PIDState pid_y_;
  PIDState pid_z_;
  PIDState pid_x_;

  // ================= 运行状态标志 =================
  bool servo_started_{false};
  bool finished_{false};

  // 三轴最新 avg（由订阅回调更新，定时器使用）
  double last_x_avg_{0.0};
  double last_y_avg_{0.0};
  double last_z_avg_{0.0};

  // ================= 回调分组/定时器 =================
  rclcpp::CallbackGroup::SharedPtr cbg_io_;
  rclcpp::CallbackGroup::SharedPtr cbg_timer_;
  rclcpp::TimerBase::SharedPtr control_timer_;

  // ================= ROS 句柄 =================
  rclcpp::Subscription<geometry_msgs::msg::Point>::SharedPtr sub_;
  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr pub_;
  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr ibvs_x_avg_pub_;
  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr ibvs_y_avg_pub_;
  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr ibvs_z_avg_pub_;

  // ================= MoveIt / TF / Servo =================
  std::shared_ptr<planning_scene_monitor::PlanningSceneMonitor> planning_scene_monitor_;
  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
  std::unique_ptr<moveit_servo::Servo> servo_;

  // ================= 滤波窗口（原始 + 中值/均值） =================
  std::deque<double> x_window_, y_window_, z_window_;
  std::deque<double> med_x_window_, med_y_window_, med_z_window_;

  // ================= 互斥锁 =================
  std::mutex buf_mtx_;

  // ================= 私有方法 =================
  // 订阅回调：接收 /result3_topic (x=-ex, y=ey, z=depth)
  void on_result3(const geometry_msgs::msg::Point::SharedPtr msg);

  // 初始化 PSM / Servo
  void setupPlanningSceneMonitor();
  void setupServo();

  // 单步 PID（带 dt）
  double pid_control_dt_(PIDState &state, double target, double current, double dt,
                         double Kp, double Ki, double Kd, double error_threshold);

  // 主控制循环（由定时器驱动）
  void control_loop();
};

} // namespace servo_visual
